#include "FrontCamHandler.h"
#include "PlateLocalization.h"
#include "mainDefs.h"


#include <queue>
using std::queue;

#include <algorithm>
using std::max;
using std::min;
#include <math.h>


#ifdef CAMERA_MODE
	#define VELOCITY_THRESHOLD 1.7
#endif
#ifdef AVI_MODE
	#define VELOCITY_THRESHOLD 0.7
#endif

#define SIZE_THRESHOLD_HEIGHT 20 /* in image size % */             
#define SIZE_THRESHOLD_WIDTH 20  /* in image size % */
#define LK_WINDOW 13
#define MEDIAN_WINDOW 25
#define NO_MOVEMENT_CONST 10
#define MOVEMENT_CONST 1
#define FRAME_SKIP_THRES 4
#define NUMBER_OF_IMAGES 1
#define FRAME_SKIP_WITH_CAR 0



void FrontCamHander::operate(){
	bool finished = false;
	
	while (!finished) {
		m_imgList.clear();
		unsigned long plateNum = 0;
		finished = waitForCar(); 
		/* waiting for a car to arrive.
		 * when we get here, we got a car.
		 */
		if (!m_imgList.empty()){
			for( t_imageList::iterator itImages = m_imgList.begin() ; itImages != m_imgList.end() ; ++itImages ){
				IplImage *tmp = m_imgList.back();
				unsigned long currentPlateNum = PlateLocalization::Instance()->FindLicenseNumber(tmp);
				if( currentPlateNum != 0) addPlateNum(currentPlateNum);
			}
			plateNum = getPlateNum();
			if( plateNum != 0) Protocol::fc2db_entered(*m_socketToDB, plateNum);
			if(!finished) finished = waitForCarExit(); // waiting for a car to arrive.
		}
		emptyPlateNum();
	}
	cvReleaseCapture(&m_camCapture);
}


bool FrontCamHander::waitForCar(){
	bool foundCar = false;
	bool carStopped = false;
	int timesNoObject = 0;
	int timesObject = 0;


	IplImage *previousImageCapture = NULL, *currentImageCapture = NULL;	

	currentImageCapture = cvQueryFrame( m_camCapture );

	cvNamedWindow("Video",0);	
	//cvNamedWindow("Thres",0);	
	//cvNamedWindow("Pic2",0);

	int frameCount = 0;
	while(!foundCar){
		++frameCount;
		if( frameCount <= FRAME_SKIP_THRES ){ 
			cvQueryFrame( m_camCapture );
			continue;
		} 
		frameCount = 0;
		cvShowImage("Video", currentImageCapture);
		if( previousImageCapture != NULL ) cvReleaseImage(&previousImageCapture);
		previousImageCapture = cvCloneImage(currentImageCapture);
		currentImageCapture = cvQueryFrame( m_camCapture );
		if( currentImageCapture == NULL ){
			cvReleaseImage(&previousImageCapture);
			return true;
		}
		cvWaitKey(10);
		if(checkForCarMovement( previousImageCapture , currentImageCapture )) ++timesObject;
		else timesObject = 0;
		if( timesObject > MOVEMENT_CONST ) foundCar = true;
	}
	while(!carStopped){
		++frameCount;
		if( frameCount <= FRAME_SKIP_THRES ){ 
			cvQueryFrame( m_camCapture );
			continue;
		} 
		frameCount = 0;
		cvShowImage("Video", currentImageCapture);
		if( previousImageCapture != NULL ) cvReleaseImage(&previousImageCapture);
		previousImageCapture = cvCloneImage(currentImageCapture);
		currentImageCapture = cvQueryFrame( m_camCapture );
		if( currentImageCapture == NULL ){
			m_imgList.push_back(cvCloneImage(previousImageCapture));
			//cvShowImage("Pic2", previousImageCapture);
			cvWaitKey(10);
			cvReleaseImage(&previousImageCapture);
			return true;
		}
		cvWaitKey(10);
		if(!checkForCarMovement( previousImageCapture , currentImageCapture )) ++timesNoObject;
		else timesNoObject = 0;
		if( timesNoObject > NO_MOVEMENT_CONST ) carStopped = true;
	}
	m_imgList.push_back(cvCloneImage(currentImageCapture));
	//cvShowImage("Pic2", currentImageCapture);
	cvReleaseImage(&previousImageCapture);
	for( int j = 1 ; j <= NUMBER_OF_IMAGES - 1 ; ++j ){
		for( int i = 1 ; i <= FRAME_SKIP_WITH_CAR ; ++i ) cvQueryFrame( m_camCapture );
		currentImageCapture = cvQueryFrame( m_camCapture );
		if( currentImageCapture == NULL ) return true;
		m_imgList.push_back(cvCloneImage(currentImageCapture));
	}
	return false;
}


bool FrontCamHander::waitForCarExit(){
	bool foundCar = false;
	bool carStopped = false;
	int timesNoObject = 0;
	int timesObject = 0;

	IplImage *previousImageCapture = NULL, *currentImageCapture = NULL;
	

	currentImageCapture = cvQueryFrame( m_camCapture );

	cvNamedWindow("Video",0);	
	//cvNamedWindow("Thres",0);	
	//cvNamedWindow("Pic2",0);



	int frameCount = 0;
	while(!foundCar){
		++frameCount;
		if( frameCount <= FRAME_SKIP_THRES ){ 
			cvQueryFrame( m_camCapture );
			continue;
		} 
		frameCount = 0;
		cvShowImage("Video", currentImageCapture);
		if( previousImageCapture != NULL ) cvReleaseImage(&previousImageCapture);
		previousImageCapture = cvCloneImage(currentImageCapture);
		currentImageCapture = cvQueryFrame( m_camCapture );
		if( currentImageCapture == NULL ){
			cvReleaseImage(&previousImageCapture);
			return true;
		}
		cvWaitKey(10);
		if(checkForCarMovement( previousImageCapture , currentImageCapture )) ++timesObject;
		else timesObject = 0;
		if( timesObject > MOVEMENT_CONST ) foundCar = true;
	}
	while(!carStopped){
		++frameCount;
		if( frameCount <= FRAME_SKIP_THRES ){ 
			cvQueryFrame( m_camCapture );
			continue;
		} 
		frameCount = 0;
		cvShowImage("Video", currentImageCapture);
		if( previousImageCapture != NULL ) cvReleaseImage(&previousImageCapture);
		previousImageCapture = cvCloneImage(currentImageCapture);
		currentImageCapture = cvQueryFrame( m_camCapture );
		if( currentImageCapture == NULL ){
			cvWaitKey(10);
			cvReleaseImage(&previousImageCapture);
			return true;
		}
		cvWaitKey(10);
		if(!checkForCarMovement( previousImageCapture , currentImageCapture )) ++timesNoObject;
		else timesNoObject = 0;
		if( timesNoObject > NO_MOVEMENT_CONST ) carStopped = true;
	}

	cvReleaseImage(&previousImageCapture);
	return false;
}




bool FrontCamHander::checkForCarMovement( IplImage* previousImageCapture ,IplImage* currentImageCapture ){
	IplImage *grayPrevious, *grayCurrent, *velocityX, *velocityY;

	IplImage *thresImage = cvCreateImage(cvGetSize(previousImageCapture), IPL_DEPTH_8U , 1);

	grayPrevious = cvCreateImage(cvGetSize(previousImageCapture), IPL_DEPTH_8U , 1);
	grayCurrent = cvCreateImage(cvGetSize(currentImageCapture), IPL_DEPTH_8U , 1);
	cvCvtColor(previousImageCapture , grayPrevious, CV_RGB2GRAY);
	cvCvtColor(currentImageCapture , grayCurrent, CV_RGB2GRAY);

	velocityX = cvCreateImage(cvSize(previousImageCapture->width, previousImageCapture->height), IPL_DEPTH_32F, 1);
	velocityY = cvCreateImage(cvSize(previousImageCapture->width, previousImageCapture->height), IPL_DEPTH_32F, 1);

	cvCalcOpticalFlowLK( grayPrevious, grayCurrent, cvSize(LK_WINDOW, LK_WINDOW), velocityX, velocityY );

	/* get ThresholdImage and do median */

	for( int i=0 ; i < velocityX->width ; ++i ){
		for( int j=0 ; j < velocityX->height ; ++j ){
			if( getVelocity(velocityX,velocityY,i,j) < VELOCITY_THRESHOLD ){
				((uchar*)thresImage->imageData + thresImage->widthStep*j)[i] = 0;
			}
			else{
				((uchar*)thresImage->imageData + thresImage->widthStep*j)[i] = 255;
			}	
		}
	}

	IplImage *thresImageFiltered = medianFilter( thresImage , MEDIAN_WINDOW );


	cvShowImage("Thres", thresImageFiltered);

	bool result = isThereBigObject( thresImageFiltered, SIZE_THRESHOLD_WIDTH, SIZE_THRESHOLD_HEIGHT, 255 );

	cvReleaseImage(&thresImage);
	cvReleaseImage(&thresImageFiltered);

	cvReleaseImage(&grayPrevious);
	cvReleaseImage(&grayCurrent);
	cvReleaseImage(&velocityX);
	cvReleaseImage(&velocityY);

	return result;
}


inline double FrontCamHander::getVelocity( IplImage* velImageX, IplImage* velImageY, int xPixel, int yPixel ){
	float velX = ((float*)(velImageX->imageData + velImageX->widthStep*yPixel))[xPixel];
	float velY = ((float*)(velImageY->imageData + velImageY->widthStep*yPixel))[xPixel];

	return sqrt(pow(velX,2)+pow(velY,2)); 

}


bool FrontCamHander::isThereBigObject( IplImage *image, int widthThreshold , int heightThreshold  , int colorThresholdLow , int colorThresholdHigh ){ //invalidates velocity images , gets grayscale image

	CvPoint startingPoint;

	getStartingPoint(image,startingPoint,colorThresholdLow,colorThresholdHigh,true); //reseting

	while( getStartingPoint(image,startingPoint,colorThresholdLow,colorThresholdHigh) ){
		queue<CvPoint> qPoints;
		qPoints.push(startingPoint);
		CvPoint topLeft = startingPoint, bottomRight = startingPoint; // bounding box
		while (!qPoints.empty()) {
			CvPoint currentPoint(qPoints.front()); 
			qPoints.pop();

			if( ((uchar*)image->imageData + image->widthStep*currentPoint.y)[currentPoint.x] < colorThresholdLow ||
				((uchar*)image->imageData + image->widthStep*currentPoint.y)[currentPoint.x] > colorThresholdHigh ) continue;

			((uchar*)image->imageData + image->widthStep*currentPoint.y)[currentPoint.x] = 0;

			/* getting the bounding box: */
			if (currentPoint.x < topLeft.x)
				topLeft.x = currentPoint.x;
			if (currentPoint.y > topLeft.y)
				topLeft.y = currentPoint.y;
			if (currentPoint.x > bottomRight.x)
				bottomRight.x = currentPoint.x;
			if (currentPoint.y < bottomRight.y)
				bottomRight.y = currentPoint.y;

			/* doing the bfs action, basically: */
			if (currentPoint.x > 0)
				qPoints.push( cvPoint(currentPoint.x - 1, currentPoint.y) );
			if (currentPoint.x + 1 < image->width)
				qPoints.push( cvPoint(currentPoint.x + 1, currentPoint.y) );
			if (currentPoint.y > 0)
				qPoints.push( cvPoint(currentPoint.x, currentPoint.y - 1) );
			if (currentPoint.y + 1 < image->height)
				qPoints.push( cvPoint(currentPoint.x, currentPoint.y + 1) );
		}
		CvSize objectSize = cvSize( bottomRight.x - topLeft.x, topLeft.y - bottomRight.y );
		if( (objectSize.width*100/image->width) >= widthThreshold && (objectSize.height*100/image->height) >= heightThreshold ) return true;
	}

	return false;
}


bool FrontCamHander::getStartingPoint( IplImage *image, CvPoint& startingPoint , int colorThresholdLow , int colorThresholdHigh , bool reset ){ //goes over image and always get a new starting point in threshold limits , gets grayscale image , should be reset between images

	static int currentX;
	static int currentY;

	if(reset){
		currentX = currentY = 0;
		return false;
	}

	for( ; currentX < image->width ; ++currentX ){
		for( ; currentY < image->height ; ++currentY ){
			if( ((uchar*)image->imageData + image->widthStep*currentY)[currentX] < colorThresholdLow ||
				((uchar*)image->imageData + image->widthStep*currentY)[currentX] > colorThresholdHigh ) continue;
			else{
				startingPoint.x = currentX;
				startingPoint.y = currentY;
				return true;
			}
		}
		currentY = 0;
	}
	return false;
}


IplImage* FrontCamHander::medianFilter( IplImage *source , int windowSize ){ 

	IplImage *outputImage = cvCreateImage(cvGetSize(source), source->depth, source->nChannels);
	windowSize = ((windowSize/2)*2) + 1 ; //fixing if window size isnt odd

	cvSmooth( source, outputImage, CV_MEDIAN, windowSize );
	return outputImage;
}




void FrontCamHander::addPlateNum( unsigned long currentPlateNum ){
	int baseNum = 1;
	for( int i = 0 ; i < NUMBERS_IN_PLATE ; ++i){
		int digit = (currentPlateNum/baseNum) % 10;
		(m_digits[i]).push_back(digit);
		baseNum*=10;
	}
	return;
}


void FrontCamHander::emptyPlateNum(){
	for( int i = 0 ; i < NUMBERS_IN_PLATE ; ++i) (m_digits[i]).clear();
	return;
}


unsigned long FrontCamHander::getPlateNum(){
	int baseNum = 1;
	unsigned long plateNum = 0;
	for( int i = 0 ; i < NUMBERS_IN_PLATE ; ++i){
		if( (m_digits[i]).size() == 0 ) return 0;
		int digitMedian = *((m_digits[i]).begin() + ((m_digits[i]).size()/2));
		plateNum += digitMedian*baseNum;
		baseNum*=10;
	}
	return plateNum;
}

